// �Զ�����Ϣ���� LhyCartesianCmd.msg
// ��msgĿ¼�´���
// float64[3] position_c
// float64 max_speed_c

// cartesian_position_controller.h
#pragma once

#include <array>
#include <memory>
#include <string>

#include <controller_interface/multi_interface_controller.h>
#include <franka_hw/franka_cartesian_command_interface.h>
#include <franka_hw/franka_state_interface.h>
#include <hardware_interface/robot_hw.h>
#include <ros/node_handle.h>
#include <ros/time.h>
#include <franka_example_controllers/LhyCartesianCmd.h> 
#include <franka_example_controllers/LhyCmdState.h> 
#include <Eigen/Dense>

namespace franka_example_controllers {

class CartesianPositionController 
    : public controller_interface::MultiInterfaceController<
          franka_hw::FrankaVelocityCartesianInterface,
          franka_hw::FrankaStateInterface> {
 public:
  bool init(hardware_interface::RobotHW* robot_hardware, 
           ros::NodeHandle& node_handle) override;
  void update(const ros::Time&, const ros::Duration& period) override;
  void starting(const ros::Time&) override;
  void stopping(const ros::Time&) override;

 private:
  void targetCallback(const franka_example_controllers::LhyCartesianCmd& msg);
  void publishStatusTimerCallback(const ros::TimerEvent& event);
  void setVelocityCmd(const Eigen::Vector3d& );
  double get_target_speed(double& error_norm);
  
  // ����������
  struct ControlParams {
    double kp = 0.5;               // P����, k^2 x < 1.4a   x<Vm /kp
    double max_speed = 0.1;     // ����ٶ�ģ�� (m/s)
    double max_acceleration = 0.1; // �����ٶ� (m/s2)
    double deadzone = 5e-4;       // λ���ݲ� (m)
  } params_;

  // ״̬����
  Eigen::Vector3d current_position_;
  Eigen::Vector3d target_position_;
  Eigen::Vector3d current_velocity_;
  double target_velocity_magnitude_{0.0};
  double global_max_speed_;
  bool has_new_target_{false};
  uint cmd_id;

  // ROS�ӿ�
  ros::Subscriber target_sub_;
  ros::Publisher status_pub_;
  ros::Timer status_timer_;
  std::unique_ptr<franka_hw::FrankaCartesianVelocityHandle> velocity_handle_;
  std::unique_ptr<franka_hw::FrankaStateHandle> state_handle_;
  ros::NodeHandle nh;
};

} // namespace franka_example_controllers

